/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include <string>

#include "mmgr.h"

#include "Matrix22.h"

/*******************************/

const Matrix22 Matrix22::ZERO     = Matrix22 (0, 0, 0, 0);
const Matrix22 Matrix22::IDENTITY = Matrix22 (1, 0, 0, 1);

/*******************************/

Matrix22::Matrix22 ()
{
}

/*******************************/

Matrix22::Matrix22 (const Matrix22& matrix)
{
	memcpy (m, matrix.m, 4 * sizeof (Real));
}

/*******************************/

Matrix22::Matrix22 (const Real m00, const Real m01, 
                    const Real m10, const Real m11) 
{
	m[0][0] = m00;
	m[0][1] = m01;
	m[1][0] = m10;
	m[1][1] = m11;
}

/*******************************/

Matrix22::~Matrix22 ()
{
}

/*******************************/

Real* Matrix22::operator [] (const unsigned int r) const
{
  return (Real*)&m[r][0];
}

/*******************************/

Matrix22::operator Real* ()
{
	return &m[0][0];
}

/*******************************/

Matrix22& Matrix22::operator = (const Matrix22& matrix)
{
	memcpy (m, matrix.m, 4 * sizeof (Real));
	
	return *this;
}

/*******************************/

Matrix22 Matrix22::operator + (const Matrix22& matrix) const
{
	Matrix22 out;
	
	for (unsigned int r = 0; r < 2; r++)
	{
		for (unsigned int c = 0; c < 2; c++)
		{
			out.m[r][c] = m[r][c] + matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix22 Matrix22::operator - (const Matrix22& matrix) const
{
	Matrix22 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		for (unsigned int c = 0; c < 2; c++)
		{
			out.m[r][c] = m[r][c] - matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix22 Matrix22::operator * (const Real scalar) const
{
	Matrix22 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		for (unsigned int c = 0; c < 2; c++)
		{
			out.m[r][c] = scalar * m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix22 operator * (const Real scalar, const Matrix22& matrix)
{
	Matrix22 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		for (unsigned int c = 0; c < 2; c++)
		{
			out.m[r][c] = scalar * matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Vector2 Matrix22::operator * (const Vector2& vector) const
{
	Vector2 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		out[r] = m[r][0]*vector[0] + m[r][1]*vector[1];
	}

	return out;
}

/*******************************/

Vector2 operator * (const Vector2& vector, const Matrix22& matrix)
{
	Vector2 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		out[r] = vector[0]*matrix.m[0][r] + vector[1]*matrix.m[1][r];
	}

	return out;
}

/*******************************/

Matrix22 Matrix22::operator * (const Matrix22& matrix) const
{
	Matrix22 out;

	for (unsigned int r = 0; r < 2; r++)
	{
		for (unsigned int c = 0; c < 2; c++)
		{
			out.m[r][c] = m[r][0]*matrix.m[0][c] + m[r][1]*matrix.m[1][c];
		}
	}

	return out;
}

/*******************************/

Matrix22 Matrix22::transpose ()
{
	return Matrix22 (m[0][0], m[1][0], 
	                 m[0][1], m[1][1]);
}

/*******************************/
